﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace BloodBot
{
    public class Pathfinding
    {
        private List<Cell> openList;
        private List<Cell> closedList;

        public const int DiagonalCost = 14;
        public const int AdjacentCost = 10;

        private Map map;

        //private int currentTotalCost;

        public Pathfinding(Map map)
        {
            this.map = map;
        }

        public LinkedList<Cell> NavigateTo(Cell startingLocation, Cell destination)
        {
            // currentLocation can't be empty
            if (startingLocation == null || destination == null)
            {
                return null;
            }

            InitializeLists();

            Cell currentLocation = startingLocation;

            while (openList.Count == 0)
            {
                openList.Add(currentLocation);
            }

            // add the adjacent cells which are navigatable to the openList
            foreach (Cell cell in map.GetCells())
            {
                if (currentLocation.IsAdjacentToCell(cell) && cell.getType().Equals("Ground"))
                {
                    openList.Add(cell);
                }
            }
            // then remove currentLocation from openList and add to the closed list
            openList.Remove(currentLocation);
            closedList.Add(currentLocation);

            while (currentLocation != destination)
            {
                // choose the lowest F score square from all those that are on the open list. Drop it from the open list and add it to the closed list.
                Cell bestOption = GetSurroundingCellWithLowestFScore(destination, currentLocation);
                if (bestOption == null)
                {
                    // if you're inside a wall or something whack like that
                    return null;
                }
                bestOption.SetParentCell(currentLocation);
                openList.Remove(bestOption);
                closedList.Add(bestOption);
                currentLocation = bestOption;

                /*
                    5) Check all of the adjacent squares. Ignoring those that are on the closed list or unwalkable 
                 * (terrain with walls, water, or other illegal terrain), add squares to the open list if they are 
                 * not on the open list already. Make the selected square the “parent” of the new squares.
                 */

                // add the adjacent cells which are navigatable to the openList

                foreach (Cell cell in map.GetCells())
                {
                    //cell.setIsPath(false);
                    if (currentLocation.IsAdjacentToCell(cell) &&
                        cell.getType().Equals("Ground"))
                    {
                        /*
                         * 6) If an adjacent square is already on the open list, check to see if this path to that square is 
                         * a better one. In other words, check to see if the G score for that square is lower if we use the
                         * current square to get there. If not, don’t do anything. 
                         * On the other hand, if the G cost of the new path is lower, change the parent of the adjacent square 
                         * to the selected square (in the diagram above, change the direction of the pointer to point at the 
                         * selected square). Finally, recalculate both the F and G scores of that square
                         */
                        if (openList.Contains(cell))
                        {
                            int travelCost;
                            if (currentLocation.IsDiagonalToCell(cell))
                            {
                                travelCost = DiagonalCost;
                            }
                            else
                            {
                                travelCost = AdjacentCost;
                            }
                            if (cell.GetCost() > currentLocation.GetCost() + travelCost)
                            {
                                cell.SetParentCell(currentLocation);
                                cell.SetCost(currentLocation.GetCost() + travelCost);
                                int heuristic = AdjacentCost * ((destination.row - cell.row) + (destination.col - cell.col));
                                if (heuristic < 0)
                                {
                                    heuristic = heuristic * -1;
                                }
                                cell.SetHeuristicCost(heuristic);
                            }

                        }
                        else if (!closedList.Contains(cell))
                        {
                            cell.SetParentCell(currentLocation);
                            openList.Add(cell);
                        }
                    }
                }
            }

            /*
             * 3) Save the path. Working backwards from the target square, go from each square to 
             * its parent square until you reach the starting square. That is your path. 
            */
            LinkedList<Cell> path = new LinkedList<Cell>();
            path.AddFirst(currentLocation);
            while (currentLocation != startingLocation)
            {
                currentLocation = currentLocation.GetParentCell();
                path.AddFirst(currentLocation);
            }
            return path;
        }

        private Cell GetSurroundingCellWithLowestFScore(Cell destination, Cell currentLocation)
        {
            // F = G + H
            // G = cost from starting point to this cell
            // H = heuristic (guess from cell to destination)

            Cell lowestCost = null;

            foreach (Cell cell in openList)
            {
                if (lowestCost == null)
                {
                    lowestCost = cell;
                }

                if (currentLocation.IsDiagonalToCell(cell))
                {
                    cell.SetCost(currentLocation.GetCost() + DiagonalCost);
                }
                else if (currentLocation.IsAdjacentToCell(cell))
                {
                    cell.SetCost(currentLocation.GetCost() + AdjacentCost);
                }

                

                // determine HEURISTIC
                // basically moves in horizontal + vertical * the cost of 1 adjacent move
                int rowCost = destination.row - cell.row;
                if (rowCost < 0)
                {
                    rowCost = rowCost * -1;
                }
                int colCost = destination.col - cell.col;
                if (colCost < 0)
                {
                    colCost = colCost * -1;
                }
                int heuristic = AdjacentCost * (rowCost + colCost);
                if (heuristic < 0)
                {
                    heuristic = heuristic * -1;
                }
                cell.SetHeuristicCost(heuristic);

                if (cell.GetCost() + cell.GetHeuristicCost() <= lowestCost.GetCost() + lowestCost.GetHeuristicCost())
                {
                    // uses last instance of lowest cost in the case that there are multiple options with same cost
                    lowestCost = cell;
                }
            }

            // return lowest cost
            if (lowestCost == null)
            {
                return null;
            }
            return lowestCost;
        }

        private void InitializeLists()
        {
            if (openList == null)
            {
                openList = new List<Cell>();
            }
            else
            {
                openList.Clear();
            }

            if (closedList == null)
            {
                closedList = new List<Cell>();
            }
            else
            {
                closedList.Clear();
            }
        }

    }
}
